#include <iostream>
#include <fstream>
#include <ctime>
#include <cstdlib> 
#include <stdio.h>
#include <time.h>

#include <fstream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp" //inclui

using namespace cv;
using std::ifstream;
using std::ofstream;

//inclui 
using namespace std;

#include "epnp.h"

 double uc = 358.01443;
   double vc = 232.09139;
   double uf = 951.26714;
   double vf = 944.58735;


//const double uc = 320;
//const double vc = 240;
//const double fu = 800;
//const double fv = 800;


// MtM takes more time than 12x12 opencv SVD with about 180 points and more:

const int n = 8; 

//double noise;

//qdouble rand(double min, double max)
//{
  //return min + (max - min) * double(rand()) / RAND_MAX;
//}

//void random_pose(Mat &c_R_w,Mat &cent_camera)
//{

//double th = -30; 
 
//Mat R(3,3,CV_64FC1);
//Mat A(3,3,CV_64FC1);
//Mat t(3,1,CV_64FC1);

//R.at<double>(0,0)=1;
//R.at<double>(0,1)=0;
//R.at<double>(0,2)=0;
//R.at<double>(1,0)=0;
//R.at<double>(1,1)=cos(th);
//R.at<double>(1,2)=-sin(th);
//R.at<double>(2,0)=0;
//R.at<double>(2,1)=sin(th);
//R.at<double>(2,2)=cos(th);

//A.at<double>(0,0)=1;
//A.at<double>(0,1)=0;
//A.at<double>(0,2)=0;
//A.at<double>(1,0)=0;
//A.at<double>(1,1)=0;
//A.at<double>(1,2)=-1;
//A.at<double>(2,0)=0;
//A.at<double>(2,1)=1;
//A.at<double>(2,2)=0;

//cent_camera.at<double>(0,0)=0.0;
//cent_camera.at<double>(0,1)=0.0;
//cent_camera.at<double>(0,2)=2.0;

  //c_R_w=R*A;
  //t=-c_R_w*cent_camera;
  
  //Mat P(3,4,CV_64FC1);

  
//P.at<double>(0,0)=R.at<double>(0,0);
//P.at<double>(0,1)=R.at<double>(0,1);
//P.at<double>(0,2)=R.at<double>(0,2);
//P.at<double>(0,3)=R.at<double>(0,1);

//P.at<double>(1,0)=R.at<double>(1,0);
//P.at<double>(1,1)=R.at<double>(1,1);
//P.at<double>(1,2)=R.at<double>(1,2);
//P.at<double>(1,3)=R.at<double>(1,0);

//P.at<double>(2,0)=R.at<double>(2,0);
//P.at<double>(2,1)=R.at<double>(2,1);
//P.at<double>(2,2)=R.at<double>(2,2);
//P.at<double>(2,3)=R.at<double>(2,0);
  
//}

//colocar z variando para termos pontos 3D
//void random_point(double & Xw, double & Yw, double & Zw)
//{
 
//  Xw = rand(1.16,3.46); //variação de x no pólígono
 // Yw = rand(-1.32,2.31); //variação de y no polígono
 // Zw = rand(0,0); 
//}



//void project_with_noise(double R[3][3], double t[3],
//			double Xw, double Yw, double Zw,
//			double & u, double & v)
//{
 // double Xc = R[0][0] * Xw + R[0][1] * Yw + R[0][2] * Zw + t[0];
  //double Yc = R[1][0] * Xw + R[1][1] * Yw + R[1][2] * Zw + t[1];
  //double Zc = R[2][0] * Xw + R[2][1] * Yw + R[2][2] * Zw + t[2];

  //double nu = rand(-noise, +noise);
  //double nv = rand(-noise, +noise);
 // u = uc + fu * Xc / Zc + nu;
 // v = vc + fv * Yc / Zc + nv;
//} //rotação dos pontos aleatórios gerados 


int main(int /*argc*/, char ** /*argv*/)

{
  
//salva em arquivo txt
  //ofstream testeopencv;
  //testeopencv.open ("/home/alex/Documentos/testeGEOMETRIAdadosreais.txt");

//testeopencv << "noise"<<",";
//testeopencv << "err2EPNP"<<",";
//testeopencv << "rot_err"<<",";
//testeopencv << "transl_err"<<",";
//testeopencv << "solvepnperr"<<",";
//testeopencv << "rot_errOCV"<<",";
//testeopencv << "transl_errOCV"<<",";
//testeopencv << "coord camera EPNP X"<<",";
//testeopencv << "coord camera EPNP Y"<<",";
//testeopencv << "coord camera EPNP Z"<<",";
//testeopencv << "coord camera OCV X"<<",";
//testeopencv << "coord camera OCV Y"<<",";
//testeopencv << "coord camera OCV Z"<<",";
//testeopencv << "timer ocv"<<",";
//testeopencv << "timer epnp"<<",";
//testeopencv<<"\n";
  
//varíaveis cvmat
  Mat rvec; 
  Mat tvec; 
  Mat matrizrvec;
  Mat jacobian;
  
  // declaração de matriz rvec C++;
  int cols = matrizrvec.cols;
  int linha = matrizrvec.rows;
  
  double matrixrvec [3][3];
  
  //declaração de matriz tvec c ++
  int j = tvec.cols;
  double vetortvec [3];
  
  //declaração do polígono
  vector<Point2f> vert (4); //point2f
  vert[0] = Point2f(1.16,-1.32);
  vert[1] = Point2f(1.16,1.32);
  vert[2] = Point2f(3.46,-2.31);
  vert[3] = Point2f(3.46,2.31);

 
  // contorno
 vector<vector<Point2f> > contours;
 
  srand(time(0));  

  double R_true[3][3], t_true[3];
  Mat R_trueocv(3,3,CV_64FC1);
  Mat t_trueocv(3,1,CV_64FC1);
  
    //random_pose(R_trueocv, t_trueocv);

 R_true[0][0]= R_trueocv.at<double>(0,0);
 R_true[0][1]= R_trueocv.at<double>(0,1);
 R_true[0][2]= R_trueocv.at<double>(0,2);
 R_true[1][0]= R_trueocv.at<double>(1,0);
 R_true[1][1]= R_trueocv.at<double>(1,1);
 R_true[1][2]= R_trueocv.at<double>(1,2);
 R_true[2][0]= R_trueocv.at<double>(2,0);
 R_true[2][1]= R_trueocv.at<double>(2,1);
 R_true[2][2]= R_trueocv.at<double>(2,2);
 
t_true[0]= t_trueocv.at<double>(0,0);
t_true[1]= t_trueocv.at<double>(1,0);
t_true[2]= t_trueocv.at<double>(2,0);

// varia noise
 //for (noise = 0.0; noise < 10.0; noise = noise + 0.5) {
 
 
   //muda os pontos aleatórios.
 //for (int conqtadr = 0; contadr < 100; contadr = contadr + 1) {
	
    epnp PnP;

  //PnP.set_internal_parameters(uc, vc, fu, fv);
  //PnP.set_maximum_number_of_correspondences(n);

//PnP.reset_correspondences();
  
  
Mat objectpointmat(n,3,CV_64FC1);//matriz dos objetos

Mat imagepointsmat(n,2,CV_64FC1); //matriz dos pontos a imagem
  
  //int ptsvalidos = 0;
//while (ptsvalidos < 10){
  //int i;
  //i = ptsvalidos;
    
 //bool Measuredist1=false;
   //double resp;
    //double Xw,Yw,Zw,u,v;
    
    
 //   random_point(Xw, Yw, Zw);
  
   // Point2f A(Xw,Yw);
   
  //  resp = pointPolygonTest (vert, A, Measuredist1);
            
    //if (resp >= 0){
    
    //cout << "\n pt: "<<Xw << " , " << Yw << " , " << Zw;
    //project_with_noise(R_true, t_true, Xw, Yw, Zw, u, v);
    //cout << " impt: "<<u << " , " << v ;
    //PnP.add_correspondence(Xw, Yw, Zw, u, v);
    
    //adicionar pontos a matriz objectpointmat
    objectpointmat.at<double>(0,0)= -0.299334;
    objectpointmat.at<double>(0,1)= 1.05641;
    objectpointmat.at<double>(0,2)= 0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(0,0)= 419;
    imagepointsmat.at<double>(0,1)= 331;

    
     objectpointmat.at<double>(1,0)= -0.29079;
    objectpointmat.at<double>(1,1)= 1.35311;
    objectpointmat.at<double>(1,2)= 0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(1,0)= 349;
    imagepointsmat.at<double>(1,1)= 318;
    
     objectpointmat.at<double>(2,0)= -0.124171;
    objectpointmat.at<double>(2,1)= 1.68343;
    objectpointmat.at<double>(2,2)=0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(2,0)=242;
    imagepointsmat.at<double>(2,1)= 312;
    
     objectpointmat.at<double>(3,0)= 0.350351 ;
    objectpointmat.at<double>(3,1)= 1.81958;
    objectpointmat.at<double>(3,2)=0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(3,0)= 219;
    imagepointsmat.at<double>(3,1)= 283;
    
     objectpointmat.at<double>(4,0)= -0.0207028;
    objectpointmat.at<double>(4,1)= 1.34984;
    objectpointmat.at<double>(4,2)=0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(4,0)= 266;
    imagepointsmat.at<double>(4,1)= 338;
    
     objectpointmat.at<double>(5,0)= 0.0606336 ;
    objectpointmat.at<double>(5,1)= 1.16142;
    objectpointmat.at<double>(5,2)=0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(5,0)= 267;
    imagepointsmat.at<double>(5,1)= 358;
    
    
     objectpointmat.at<double>(6,0)= -0.0551719;
    objectpointmat.at<double>(6,1)= 2.99749;
    objectpointmat.at<double>(6,2)= 0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(6,0)= 299;
    imagepointsmat.at<double>(6,1)= 307;
    
     objectpointmat.at<double>(7,0)= -0.237592;
    objectpointmat.at<double>(7,1)= 1.9264;
    objectpointmat.at<double>(7,2)=0;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(7,0)= 254;
    imagepointsmat.at<double>(7,1)= 289;
    
   
    //ptsvalidos = ptsvalidos + 1;
   // }

//}

 double m[3][3] = {{uf,0,uc}, {0,vf,vc}, {0,0,1}};
 Mat camera = Mat(3, 3, CV_64FC1, m);
 double d[4] = {0,-0.41348,-0.00386,0.00493};
 Mat dist = Mat(1, 4, CV_64FC1, d);

  // COLOCAR TIMER!!!!
 //double t1 = getTickCount();
 
 cv::solvePnP(objectpointmat,imagepointsmat,camera,dist,rvec,tvec,false);   
 
 //double t3 = (getTickCount () - t1)/getTickFrequency(); 
 
 double R_est[3][3], t_est[3];
  
  
  //colocar timer
// double t2 = getTickCount(); 
// double err2 = PnP.compute_pose(R_est, t_est);
//double t4 = (getTickCount()- t2)/getTickFrequency();  
 double rot_err, transl_err;
  
 
 // PnP.relative_error(rot_err, transl_err, R_true, t_true, R_est, t_est);
 // cout << ">>> Reprojection error: " << err2 << endl;
 // cout << ">>> rot_err: " << rot_err << ", transl_err: " << transl_err << endl;
 // cout << endl;

 // double errtrue = PnP.reprojection_error(R_true, t_true); 
 // cout << "'True reprojection error':"
  // << errtrue << endl;
 // cout << endl;
  
 // cout << "True pose:" << endl;
 // PnP.print_pose(R_true, t_true);
  //cout << endl;
  //cout << "Found pose:" << endl;
  //PnP.print_pose(R_est, t_est);
  
  //cout << "\n*****OCV*****"; 
 // cout << "\ncamera"; 
 // cout << camera;
 // cout << "\nobjpts"; 
  //cout << objectpointmat;
  //cout << "\nimgpts"; 
  //cout << imagepointsmat;
  //cout << "\n rvec" << endl;
  //cout << rvec;
  //cout << "\n tvec" << endl;
  //cout << tvec;

//  transforma o rvec em uma matriz.
  
  cv::Rodrigues(rvec,matrizrvec,jacobian);
  
 // cout <<"\n matrizrvec"<<endl;
 // cout << matrizrvec;
  
  //coloca elementos na matriz c++ 
 // cout <<"\n matrixrvec c++" <<endl;
  
  for (linha = 0; linha < 3; linha++) {
      
    for (cols = 0; cols < 3; cols++) {
      
      matrixrvec[linha][cols] = matrizrvec.at<double> (linha,cols);
 
       
 //     cout<< matrixrvec[linha][cols];
    
   //  cout<<"\n"<< endl;
    }
  
  }
  
  //colocar elementos na matriz tvec c++
 // cout <<"\n matrixtvec c++" <<endl;
   for (j = 0; j < 3; j++) {
      
      vetortvec[j] = tvec.at<double> (0,j);

   //     cout<<vetortvec[j];
    
    // cout<<"\n"<< endl;      
    }
  
 double solvepnperr = PnP.reprojection_error(matrixrvec, vetortvec);
  
   cout << "SolvePNP reprojection error"
       << solvepnperr << endl;
  
  // cout << "\n";
 
//erro relativo do solvePnP

  double rot_errOCV;
  double transl_errOCV;
  PnP.relative_error(rot_errOCV, transl_errOCV, R_true,t_true, matrixrvec, vetortvec);

 // cout << ">>> Reprojection error OCV: " << err2 << endl;
 // cout << ">>> rot_errOCV: " << rot_errOCV << ", transl_errOCV: " << transl_errOCV << endl;
 // cout << endl;
  // guarda os dados.
  
 
//testeopencv <<noise<<",";

//testeopencv << err2<<",";

//testeopencv << rot_err<<",";

//testeopencv << transl_err<<",";

//testeopencv << solvepnperr<<",";

//testeopencv << rot_errOCV<<",";

//testeopencv << transl_errOCV<<",";   

//testeopencv <<t_est[0]<<",";

//testeopencv <<t_est[1]<<",";

//testeopencv <<t_est[2]<<",";

//testeopencv <<vetortvec[0]<<",";

//testeopencv <<vetortvec[1]<<",";

//testeopencv <<vetortvec[2]<<",";

//testeopencv << t3<<",";

//testeopencv << t4<<",";

//testeopencv<<"\n";
  
//} //fim da variação da const
  //   }//fim da variaçãpo do noise

//testeopencv.close();
return 0;
 
  
}

